function [ u_h condK ] = CantileverBeam_Solver_BC_penalty( dp_efg, s_near, x_nodes, x_samples, w_samples, parameters, options,penalty)
%CANTILEVERBEAM_SOLVER_BC_DIRECT This function assembled the stiffness
%     matrix K and the right hand side rhs corresponding to the cantilever
%     beam problem explained in Timoshenko's book.
%
% Input
%    s_near    : list of neighbors
%    x_nodes   : node points
%    x_samples : sample point
%    w_samples : gauss weigth for each sample point
%    parameters: L (length), D (diameter), nu (Poisson coefficient), E
%                (Young modulus)
%    options   : lme options
%
% Output:
%    u_h     : vectorial displacement field
%

% Material parameters
E  = parameters.E;
nu = parameters.nu;
P  = parameters.P;
D  = parameters.D;
L  = parameters.L;
% Number of nodes and gaussian points
nPts = size(x_nodes,1);
sPts = size(x_samples,1);

mockTheAgency = 0;

%% ------------------------------------------------------------------------
% The rhs is initialized
rhs    = zeros(2*nPts,1);
I = D^3/12;
%% The BCs nodes are classified
ids   = (1:nPts);
ind_Dirichlet = zeros(2,1);
% (x=0)
ind_bc1 = ids(abs(x_nodes(:,1))<1.e-6);
% (x=L)
ind_bc2 = ids(abs(x_nodes(:,1)-L)<1.e-6);
% (y=0)
ind_bc3 = ids(abs(x_nodes(:,2))<1.e-6);
% (y=d/2)
ind_bc4 = ids(abs(x_nodes(:,2)-(D/2))<1.e-6);
%(x=0,y=0)
ind_Dirichlet(1) = ids(abs(x_nodes(:,1))<1.e-6 & abs(x_nodes(:,2))<1.e-6);
%(x=0,y=D/2)
ind_Dirichlet(2) = ids(abs(x_nodes(:,1))<1.e-6 & abs(x_nodes(:,2)-0.5*D)<1.e-6);

% options for 1D integration and 1D EFG
opt_int1D.orderGL = 30;
opt_int1D.pruning = 0;
options1D = options;
options1D.dim  = 2;
options1D.grad = 0;

%% Boundary nodes corresponding to x=0
xNodes_bc1_X0          = x_nodes(ind_bc1,:);
[xGauss_bc1_X0y w_s1D] = MakeGLSamples1D(xNodes_bc1_X0(:,2), opt_int1D);
xGauss_bc1_X0 = zeros(size(xGauss_bc1_X0y,1),2);
xGauss_bc1_X0(:,2) = xGauss_bc1_X0y(:);
% adjacency structure with the nearest neighbors nodes to each sample point
if (mockTheAgency == 0)
    bc_s_near = SamplesAdjacency_efg(x_nodes, xGauss_bc1_X0, options.range_n);
else
    bc_mockSnear = cell(1, size(xGauss_bc1_X0,1));
    for sx = 1 : size(xGauss_bc1_X0,1);
        bc_mockSnear(sx) = {(1:size(x_nodes,1))};
    end;
    bc_s_near = bc_mockSnear;
end;

options1D = options;
options1D.s_near = bc_s_near;
outEFG  = wrapperCHINO(x_nodes, xGauss_bc1_X0, options1D);
p_efg1D = outEFG.p_samp;

for i = 1 : size(xGauss_bc1_X0)
  y_s      = xGauss_bc1_X0(i,2);
  w_s      = w_s1D(i);
  i_nears  = bc_s_near{i};
  p_nears  = p_efg1D{i};
  for a = 1 : size(i_nears);
%      if (i_nears(a) ~= ind_Dirichlet(1) && i_nears(a) ~= ind_Dirichlet(2));
        rhs(2*i_nears(a)-1) = rhs(2*i_nears(a)-1) + P*(L/I)*y_s*p_nears(a)*w_s;           %%% sigma_xx con x=0
        rhs(2*i_nears(a))   = rhs(2*i_nears(a))   - P/(2*I)*(D^2/4-y_s^2)*p_nears(a)*w_s; %%% sigma_xy
%      end;
  end;
end


%% Boundary nodes corresponding to x=L
xNodes_bc2_XL          = x_nodes(ind_bc2,:);
[xGauss_bc2_XLy w_s1D] = MakeGLSamples1D(xNodes_bc2_XL(:,2), opt_int1D);
xGauss_bc2_XL = L*ones(size(xGauss_bc2_XLy,1),2);
xGauss_bc2_XL(:,2) = xGauss_bc2_XLy(:);
% adjacency structure with the nearest neighbors nodes to each sample point
%bc_s_near = SamplesAdjacency_efg(x_nodes, xGauss_bc2_XL, options.range_n);
if (mockTheAgency == 0)
    bc_s_near = SamplesAdjacency_efg(x_nodes, xGauss_bc2_XL, options.range_n);
else
    bc_mockSnear = cell(1, size(xGauss_bc2_XL,1));
    for sx = 1 : size(xGauss_bc2_XL,1);
        bc_mockSnear(sx) = {(1:size(x_nodes,1))};
    end;
    bc_s_near = bc_mockSnear;
end;
% Local-max entropy basis functions computation
options1D.s_near = bc_s_near;
outEFG  = wrapperCHINO(x_nodes, xGauss_bc2_XL, options1D);
p_efg1D = outEFG.p_samp;

for i = 1 : size(xGauss_bc2_XL,1)
  y_s      = xGauss_bc2_XL(i,2);
  w_s      = w_s1D(i);
  i_nears  = bc_s_near{i};
  p_nears  = p_efg1D{i};
  for a = 1 : size(i_nears);
      rhs(2*i_nears(a)) = rhs(2*i_nears(a)) + P/(2*I)*(D^2/4-y_s^2)*p_nears(a)*w_s; %%% -sigma_xy
  end;
end

%% Boundary nodes corresponding to y=0
xNodes_bc3_Y0       = x_nodes(ind_bc3,:);
[xGauss_bc3_Y0x w_s1D] = MakeGLSamples1D(xNodes_bc3_Y0(:,1), opt_int1D);
xGauss_bc3_Y0=zeros(size(xGauss_bc3_Y0x,1),2);
xGauss_bc3_Y0(:,1) = xGauss_bc3_Y0x(:);
% adjacency structure with the nearest neighbors nodes to each sample point
%bc_s_near = SamplesAdjacency_efg(x_nodes, xGauss_bc3_Y0, options.range_n);
if (mockTheAgency == 0)
    bc_s_near = SamplesAdjacency_efg(x_nodes, xGauss_bc3_Y0, options.range_n);
else
    bc_mockSnear = cell(1, size(xGauss_bc3_Y0,1));
    for sx = 1 : size(xGauss_bc3_Y0,1);
        bc_mockSnear(sx) = {(1:size(x_nodes,1))};
    end;
    bc_s_near = bc_mockSnear;
end;

% Local-max entropy basis functions computation
options1D.s_near = bc_s_near;
outEFG  = wrapperCHINO(x_nodes, xGauss_bc3_Y0, options1D);
p_efg1D = outEFG.p_samp;

for i = 1 : size(xGauss_bc3_Y0,1);
  w_s      = w_s1D(i);
  i_nears  = bc_s_near{i};
  p_nears  = p_efg1D{i};
  for a = 1 : size(i_nears);
%      if (i_nears(a) ~= ind_Dirichlet(1) && i_nears(a) ~= ind_Dirichlet(2));
        rhs(2*i_nears(a)-1) = rhs(2*i_nears(a)-1) - P/(2*I)*D^2/4*p_nears(a)*w_s;   %%%sigma xy con y=0
%      end;
  end;
end
% 
% %% Boundary nodes corresponding to y=D/2
% xNodes_bc4_YD          = x_nodes(ind_bc4,:);
% [xGauss_bc4_YDx w_s1D] = MakeGLSamples1D(xNodes_bc4_YD(:,1), opt_int1D);
% xGauss_bc4_YD = (D/2) * ones(size(xGauss_bc4_YDx,1),2);
% xGauss_bc4_YD(:,1) = xGauss_bc4_YDx(:);
% 
% % adjacency structure with the nearest neighbors nodes to each sample point
% if (mockTheAgency == 0)
%     bc_s_near = SamplesAdjacency_efg(x_nodes, xGauss_bc4_YD, options.range_n);
% else
%     bc_mockSnear = cell(1, size(xGauss_bc4_YD,1));
%     for sx = 1 : size(xGauss_bc4_YD,1);
%         bc_mockSnear(sx) = {(1:size(x_nodes,1))};
%     end;
%     bc_s_near = bc_mockSnear;
% end;
% 
% options1D = options;
% options1D.s_near = bc_s_near;
% outEFG  = wrapperCHINO(x_nodes, xGauss_bc4_YD, options1D);
% p_efg1D = outEFG.p_samp;
% 
% for i = 1 : size(xGauss_bc4_YD,1)
%   x_s      = xGauss_bc4_YD(i,1);
%   w_s      = w_s1D(i);
%   i_nears  = bc_s_near{i};
%   p_nears  = p_efg1D{i};
%   for a = 1 : size(i_nears);
%       %if (i_nears(a) ~= ind_Dirichlet(1) && i_nears(a) ~= ind_Dirichlet(2));
%         rhs(2*i_nears(a)) = rhs(2*i_nears(a)) + P*((L-x_s)/I)*(D/2)*p_nears(a)*w_s;    %%% sigma_xx con y=D/2
%       %end;
%   end;
% end


%% ------------------------------------------------------------------------
%  The stiffness matrix is assembled 
K = zeros(2*nPts,2*nPts);

% estado plano de tensiones
Ebar = E;
nuBar = nu;

% Estado plano de deformaciones
% Ebar = E/(1-nu^2);
% nuBar = nu/(1-nu);

% matriz de stifler
C_stiff = Ebar/(1-nuBar^2)*[    1, nuBar,           0 ;...
 	                        nuBar,     1,           0 ;...
 	                          0,       0, (1-nuBar)/2];


% asi lo tenia daniel
% C_stiff=E/(1+nu)/(1-2*nu)*[1-nu,   nu,          0 ;...
% 	                         nu, 1-nu,          0 ;...
% 	                          0,    0, (1-2*nu)/2];

for ig=1:sPts
  nact = length(s_near{ig});
  B_ig = zeros(3, 2*nact);
  B_ig(1,1:2:2*nact) = dp_efg{ig}(:,1)';
  B_ig(2,2:2:2*nact) = dp_efg{ig}(:,2)';
  B_ig(3,2:2:2*nact) = dp_efg{ig}(:,1)';
  B_ig(3,1:2:2*nact) = dp_efg{ig}(:,2)';
  K_ig_loc = B_ig'*C_stiff*B_ig;

  %assembly
  active=s_near{ig};
  K(2*active(:)-1,2*active(:)-1) = ...
      K(2*active(:)-1,2*active(:)-1) + ...
      K_ig_loc(1:2:2*nact,1:2:2*nact)*w_samples(ig);
  K(2*active(:),2*active(:)-1) = ...
      K(2*active(:),2*active(:)-1) + ...
      K_ig_loc(2:2:2*nact,1:2:2*nact)*w_samples(ig);
  K(2*active(:)-1,2*active(:)) = ...
      K(2*active(:)-1,2*active(:)) + ...
      K_ig_loc(1:2:2*nact,2:2:2*nact)*w_samples(ig);
  K(2*active(:),2*active(:)) = ...
      K(2*active(:),2*active(:)) + ...
      K_ig_loc(2:2:2*nact,2:2:2*nact)*w_samples(ig);
end

%% Sums the contribution of the penalty parameter on the system matrix-----
%rueditas en todos los nodos del borde
% [xGauss_bc1_X0y w_s1D] = MakeGLSamples1D(xNodes_bc1_X0(:,2), opt_int1D);
% xGauss_bc1_X0 = zeros(size(xGauss_bc1_X0y,1),2);
% xGauss_bc1_X0(:,2) = xGauss_bc1_X0y(:);
% bc_s_near = SamplesAdjacency_efg(x_nodes, xGauss_bc1_X0, options.range_n);
% options1D.s_near = bc_s_near;
% outEFG  = wrapperCHINO(x_nodes, xGauss_bc1_X0, options1D);
% p_efgBC = outEFG.p_samp;
% 
% for iBCGP = 1 : size(xGauss_bc1_X0);
%     w_s      = w_s1D(iBCGP);
%     i_nears  = bc_s_near{iBCGP};
%     p_nears  = p_efgBC{iBCGP};
%     for a = 1 : size(i_nears);
%         ind = i_nears(iBCGP);
%         K(2*ind-1, 2*ind-1) = K(2*ind-1, 2*ind-1) +...
%             penalty * p_nears(a)*p_nears(a)*w_s;
%         K(2*ind, 2*ind) = K(2*ind, 2*ind) +...
%             penalty * p_nears(a)*p_nears(a)*w_s;
%     end;    
% end;

M = zeros(size(K));
%(x=0,y=0)  ux=0 uy=0
%contribution on system matrix
ind1 = ind_Dirichlet(1);
ind2 = ind_Dirichlet(2);
auxGPonNode = zeros(2,2);
auxGPonNode(1,:) = x_nodes(ind1,:);
auxGPonNode(2,:) = x_nodes(ind2,:);
%bc_s_near = SamplesAdjacency_efg(x_nodes, auxGPonNode, options.range_n);
if (mockTheAgency == 0)
    bc_s_near = SamplesAdjacency_efg(x_nodes, auxGPonNode, options.range_n);
else
    bc_mockSnear = cell(1, size(auxGPonNode,1));
    for sx = 1 : size(auxGPonNode,1);
        bc_mockSnear(sx) = {(1:size(x_nodes,1))};
    end;
    bc_s_near = bc_mockSnear;
end;

options1D.s_near = bc_s_near;
outEFG  = wrapperCHINO(x_nodes, auxGPonNode, options1D);

% para el ind1
p_efg = outEFG.p_samp{1};
s_near = bc_s_near{1};
for a = 1 : size(s_near);
    for b = 1 : size(s_near);
        inda = s_near(a);
        indb = s_near(b);
        pa = p_efg(a);
        pb = p_efg(b);        
        M(2*inda-1,2*indb-1) = M(2*inda-1,2*indb-1) + pa*pb;
        M(2*inda,2*indb)     = M(2*inda,2*indb)     + pa*pb;
    end;
end;

% para el ind1
p_efg = outEFG.p_samp{2};
s_near = bc_s_near{2};
for a = 1 : size(s_near);
    for b = 1 : size(s_near);
        inda = s_near(a);
        indb = s_near(b);
        pa = p_efg(a);
        pb = p_efg(b);        
        M(2*inda-1,2*indb-1) = M(2*inda-1,2*indb-1) + pa*pb;
    end;
end;

K = K + penalty * M;

%(x=0,y=0)  ux=0 uy=0
%contribution on system matrix
% ind = ind_Dirichlet(1);
% auxGPonNode = zeros(2,2);
% auxGPonNode(1,:) = x_nodes(ind,:);% + [0.00001,0.00001];
% auxGPonNode(2,:) = x_nodes(ind,:) + [0.0001,0.0001];
% bc_s_near = SamplesAdjacency_efg(x_nodes, auxGPonNode, options.range_n);
% options1D.s_near = bc_s_near;
% outEFG  = wrapperCHINO(x_nodes, auxGPonNode, options1D);
% p_efg = outEFG.p_samp{1};
% K(2*ind-1,2*ind-1) = K(2*ind-1,2*ind-1) + penalty * p_efg(1)*p_efg(1);
% K(2*ind,2*ind)     = K(2*ind,2*ind)     + penalty * p_efg(1)*p_efg(1);
%contribution on rhs vector
%rhs(2*ind-1)       = 0;
%rhs(2*ind)         = 0;

%(x=0,y=D/2)  ux=0
% ind = ind_Dirichlet(2);
% auxGPonNode = zeros(2,2);
% auxGPonNode(1,:) = x_nodes(ind,:);
% auxGPonNode(2,:) = x_nodes(ind,:) + [0.0001,0.0001];
% bc_s_near = SamplesAdjacency_efg(x_nodes, auxGPonNode, options.range_n);
% options1D.s_near = bc_s_near;
% outEFG  = wrapperCHINO(x_nodes, auxGPonNode, options1D);
% p_efg = outEFG.p_samp{1};
% 
% K(2*ind-1,2*ind-1) = K(2*ind-1,2*ind-1) + penalty * p_efg(1)*p_efg(1);
%rhs(2*ind-1)       = 0;


% 
% %(x=0,y=0)  ux=0 uy=0
% ind = ind_Dirichlet(1);
% K(2*ind,:)         = 0;
% %K(:,2*ind)         = 0;
% rhs(2*ind)         = 0;
% K(2*ind,2*ind)     = 1;
% K(2*ind-1,:)       = 0;
% %K(:,2*ind-1)       = 0;
% rhs(2*ind-1)       = 0;
% K(2*ind-1,2*ind-1) = 1;
% 
% %(x=0,y=D/2)  ux=0
% ind = ind_Dirichlet(2);
% K(2*ind-1,:)       = 0;
% %K(:,2*ind-1)       = 0;
% rhs(2*ind-1)       = 0;
% K(2*ind-1,2*ind-1) = 1;
%rhs=rhs*1.2;
save('RHS_efg_h5.mat', 'rhs');
%load('RHS_lme_h5.mat', 'rhs');

%% ------------------------------------------------------------------------
% The system is solved
u_h = K \ rhs;
condK = cond(K);

end
